/*********************************************************************
* \_file WaypointAnalyser.cpp
*
* Implements the waypoint _file inspection tool.
*
* \author Instituto Superior Tecnico de Lisboa, Portugal
* \author Technical University of Lisbon, Portugal
* 
* \author Agentes Autonomos e Sistemas Multiagente
* \author Autonomous Agents and Multiagent Systems
* 
* \version	1.0
* \date		2006/2007
*********************************************************************/
#include "WaypointAnalyser.h"
using namespace util::tools;

#define TIMEOUT_NOT_DEFINED -1

#include "../debug.h"
#include "../util.h"
using namespace util;

#include <time.h>

WaypointAnalyser::WaypointAnalyser(edict_t *enity, int team, int model)
:Agent(enity, team, model)
{
	collisionHandler = new WA_CollisionHandler(this);
	getCollisionManager().setCollisionHandler(collisionHandler);
	_validateUnreachables = new WA_UnreachablesState(this);
	_validateConnections = new WA_ConnectionsState(this);

	newRound();
}

WaypointAnalyser::~WaypointAnalyser()
{
	if(collisionHandler != NULL)
		delete collisionHandler;
	if(_validateUnreachables != NULL)
		delete _validateUnreachables;
	if(_validateConnections != NULL)
		delete _validateConnections;
}
void WaypointAnalyser::newRound()
{
	roundEndTime = CVAR_GET_FLOAT("mp_roundtime");
	roundEndTime *= 60;
	roundEndTime += CVAR_GET_FLOAT("mp_freezetime");
	roundEndTime += getWorld().getTime();

	collisionHandler->newRound();
	_validateUnreachables->newRound();
	_validateConnections->newRound();
	Agent::newRound();
}
void WaypointAnalyser::think()
{
	_validateUnreachables->update();
	_validateConnections->update();

	ICommunication::MessageRecord record;
	if(communication->receiveMsg(ICommunication::SayMessages::sayMsg, &record))
		handleMessage(record);
	else if(communication->receiveMsg(ICommunication::SayMessages::sayTeamMsg, &record))
		handleMessage(record);
	else if(communication->receiveMsg(ICommunication::SayMessages::sayDeadMsg, &record))
		handleMessage(record);
}
void WaypointAnalyser::handleMessage(ICommunication::MessageRecord& record)
{
	char msgBuffer[512];
	char command[32];
	char arg1[32];
	char arg2[32];

	strcpy(arg1, "");
	strcpy(arg2, "");
	sscanf(record.msg, "%s %s %s", command, arg1, arg2);

	if((strcmp(command, "moveTo")==0)||(strcmp(command, "m")==0))
	{
		if(util::isNullString(arg1))
		{
			util::debug::printToServer("ERROR: Incorrect arguments. Syntax: moveTo <destinationWaypoint>");
			return;
		}

		int destination = atoi(arg1);
		getWaypointNavigation().moveToWaypoint(destination);
		sprintf(msgBuffer, "Moving to waypoint %d.\n", destination);
		communication->sayText(msgBuffer);
	}
	else if((strcmp(command,"moveToHere")==0) || (strcmp(command, "mH")==0))
	{
		Vector position = g_threatTab[0].origin;
		getFreeNavigation().moveToDestination(position);
		sprintf(msgBuffer, "Moving towards your current position (%.2f %.2f %.2f)...\n", 
			position.x, position.y, position.z);
		communication->sayText(msgBuffer);
	}
	else if((strcmp(command,"stop")==0) || (strcmp(command, "s")==0))
	{
		getWaypointNavigation().stop();
		_validateUnreachables->stop();
		_validateConnections->stop();
		sprintf(msgBuffer, "Stopped.\n");
		communication->sayText(msgBuffer);
	}
	else if((strcmp(command, "findDeadends")==0)||(strcmp(command, "fD")==0))
	{
		sprintf(msgBuffer, "Found %d deadends. A detailed report was printed on the console.\n", findDeadends());
		communication->sayText(msgBuffer);
	}
	else if((strcmp(command, "findUnreachables")==0)||(strcmp(command, "fU")==0))
	{
		int nUnreachables;
		if(!util::isNullString(arg1))
			nUnreachables = findUnreachables(atoi(arg1));
		else
			nUnreachables = findUnreachables(0);
		sprintf(msgBuffer, "Found %d unreachables. A detailed report was printed on the console.\n", nUnreachables);
		communication->sayText(msgBuffer);
	}
	else if((strcmp(command, "findAndValidateUnreachables")==0)||(strcmp(command, "fvU")==0))
	{
		if(!util::isNullString(arg1) && !util::isNullString(arg2))
			findAndValidateUnreachables(atoi(arg1), atoi(arg2));
		else if(!util::isNullString(arg1))
			findAndValidateUnreachables(atoi(arg1), getWaypointNavigation().getNumberOfWaypoints()-1);
		else
			findAndValidateUnreachables(0, getWaypointNavigation().getNumberOfWaypoints()-1);
		sprintf(msgBuffer, "Finding and validating unreachebles...\n");
		communication->sayText(msgBuffer);
	}
	else if((strcmp(command, "validateConnections")==0)||(strcmp(command, "vC")==0))
	{
		if(!util::isNullString(arg1) && !util::isNullString(arg2))
			validateConnections(atoi(arg1), atoi(arg2));
		else if(!util::isNullString(arg1))
			validateConnections(atoi(arg1), getWaypointNavigation().getNumberOfWaypoints()-1);
		else
			validateConnections(0, getWaypointNavigation().getNumberOfWaypoints()-1);
		sprintf(msgBuffer, "Validating connections...\n");
		communication->sayText(msgBuffer);
	}
}

int WaypointAnalyser::findDeadends()
{
	Waypoint** waypoints = getWaypointNavigation().getWaypoints();

	bool isDeadend;
	int nWaypoints = getWaypointNavigation().getNumberOfWaypoints();
	list<int> deadends;
	for(int i=0; i<nWaypoints; i++)
	{
		isDeadend = true;
		for (int j = 0; j < MAX_PATH_INDEX; j++)
		{
			if(waypoints[i]->index[j] != -1)
			{
				isDeadend = false;
				break;
			}
		}

		if(isDeadend)
			deadends.push_back(i);
	}
	WA_Results results;
	results.toFile("deadends", "deadends", &deadends);

	return deadends.size();
}
int WaypointAnalyser::findUnreachables(int startIndex)
{
	list<int> closed;

	findUnreachables(startIndex, &closed);

	list<int> unreachables;
	Waypoint** waypoints = getWaypointNavigation().getWaypoints();
	int nWaypoints = getWaypointNavigation().getNumberOfWaypoints();
	for(int i=0; i<nWaypoints; i++)
	{
		bool isClosed = false;

		list<int>::iterator it;
		for(it = closed.begin(); it != closed.end(); it++)
			if((*it) == i)
			{
				isClosed = true;
				break;
			}

			if(!isClosed)
				unreachables.push_back(i);
	}

	WA_Results results;
	results.toFile("unreachables", "unreachables", &unreachables);

	return unreachables.size();
}
void WaypointAnalyser::findUnreachables(int currentIndex, list<int>* closed)
{
	Waypoint* current = getWaypointNavigation().getWaypoints()[currentIndex];
	closed->push_back(currentIndex);

	int childIndex;
	for(int i=0; i<MAX_PATH_INDEX; i++)
	{
		childIndex = current->index[i];
		bool isClosed = false;
		if(childIndex != -1)
		{
			list<int>::iterator it = closed->begin();
			while(it != closed->end())
			{
				if((*it) == childIndex)
				{
					isClosed = true;
					break;
				}
				it++;
			}
			if(!isClosed)
				findUnreachables(childIndex, closed);
		}
	}
}
void WaypointAnalyser::stopValidations()
{
	_validateUnreachables->stop();
	_validateConnections->stop();
}
void WaypointAnalyser::findAndValidateUnreachables(int startIndex, int endIndex)
{
	stopValidations();
	_validateUnreachables->start(startIndex, endIndex);
}
void WaypointAnalyser::validateConnections(int startIndex, int endIndex)
{
	stopValidations();
	_validateConnections->start(startIndex, endIndex);
}
void WA_Results::openFile(const char* suffix)
{
	char buffer[256];

	util::getModDirectory(buffer);
	strcat(buffer, "logs/");
	strcat(buffer, STRING(gpGlobals->mapname));
	strcat(buffer, "_");
	strcat(buffer, suffix);
	strcat(buffer, "_");

	char tmpbuf[128];
	__time64_t ltime;
	struct tm *today;

	_time64( &ltime );
	today = _localtime64( &ltime );
	strftime( tmpbuf, 128, "%d_%b_%y_%H_%M_%S", today );
	strcat(buffer, tmpbuf);
	strcat(buffer, ".txt");

	_file = fopen(buffer, "wb");
}
void WA_Results::closeFile()
{
	fclose(_file);
}
void WA_Results::write(const char* str)
{
	util::debug::printToServer(str);
	fwrite(str, strlen(str), 1, _file);
	fflush(_file);
}
void WA_Results::write(const char* name, list<int>* waypoints)
{
	char msg[256];
	sprintf(msg, "\t%s waypoints: ", name, waypoints->size()); write(msg);
	list<int>::iterator it;
	for(it = waypoints->begin(); it != waypoints->end(); it++)
	{
		sprintf(msg, "%d ", *it); 
		write(msg);
	}
}
void WA_Results::write(list<WA_Connection*>* connections)
{
	char msg[256];
	Waypoint** waypoints = g_waypoints;
	list<WA_Connection*>::iterator it;
	for(it = connections->begin(); it != connections->end(); it++)
	{
		if(waypoints[(*it)->waypoint]->flags & WaypointFlags::goal)
		{
			sprintf(msg, "\t%d (Goal) -> %d\r\n", (*it)->waypoint, (*it)->neighbor); 
			write(msg);
		}
		else
		{
			sprintf(msg, "\t%d -> %d\r\n", (*it)->waypoint, (*it)->neighbor); 
			write(msg);
		}
	}
}
void WA_Results::toFile(
		const char* fileNameSuffix, const char* itemName, list<int>* selectedWaypoints)
{
	openFile(fileNameSuffix);

	char msg[256];
	write("**********************************\r\n");
	sprintf(msg, "Report for %s\r\n", itemName);
	write(msg);
	write("**********************************\r\n");

	Waypoint** waypoints = g_waypoints;
	list<int> goalWaypoints;
	list<int> normalWaypoints;
	list<int>::iterator it;
	for(it = selectedWaypoints->begin(); it != selectedWaypoints->end(); it++)
	{
		if(waypoints[*it]->flags & WaypointFlags::goal)
			goalWaypoints.push_back(*it);
		else
			normalWaypoints.push_back(*it);
	}

	goalWaypoints.sort();
	normalWaypoints.sort();

	sprintf(msg, "Summary: Found %d %s (Normal: %d Goal: %d)\r\n", 
		selectedWaypoints->size(), itemName, normalWaypoints.size(), goalWaypoints.size()); write(msg);

	sprintf(msg, "\r\nDetails:\r\n"); write(msg);
	write("All", selectedWaypoints); write("\r\n");
	write("Goal", &goalWaypoints); write("\r\n");
	write("Normal", &normalWaypoints); write("\r\n");

	write("----------------**----------------\r\n");

	closeFile();
}
void WA_Results::toFile(
		const char* fileNameSuffix, const char* noItemsName, const char* yesItemsName, 
			list<int>* noWaypoints, list<int>* yesWaypoints)
{
	openFile(fileNameSuffix);

	char msg[256];
	write("**********************************\r\n");
	sprintf(msg, "Report for %s\r\n", noItemsName);
	write(msg);
	write("**********************************\r\n");

	Waypoint** waypoints = g_waypoints;
	list<int> goalWaypoints;
	list<int> normalWaypoints;
	list<int>::iterator it;
	for(it = noWaypoints->begin(); it != noWaypoints->end(); it++)
	{
		if(waypoints[*it]->flags & WaypointFlags::goal)
			goalWaypoints.push_back(*it);
		else
			normalWaypoints.push_back(*it);
	}

	goalWaypoints.sort();
	normalWaypoints.sort();

	sprintf(msg, "Summary: \r\n\tFound %d %s (Normal: %d Goal: %d). \r\n\tFound %d %s.\r\n", 
		noWaypoints->size(), noItemsName, normalWaypoints.size(), goalWaypoints.size(), 
		yesWaypoints->size(), yesItemsName); write(msg);

	sprintf(msg, "\r\nDetails (%s):\r\n", noItemsName); write(msg);
	write("All", noWaypoints); write("\r\n");
	write("Goal", &goalWaypoints); write("\r\n");
	write("Normal", &normalWaypoints); write("\r\n");
	sprintf(msg, "\r\nDetails (%s):\r\n", yesItemsName); write(msg);
	write("All", yesWaypoints); write("\r\n");

	write("----------------**----------------\r\n");

	closeFile();
}
void WA_Results::toFile(
		const char* fileNameSuffix, const char* noItemName, const char* yesItemName, 
		list<WA_Connection*>* noConnections, list<WA_Connection*>* yesConnections)
{
	openFile(fileNameSuffix);

	char msg[256];
	write("**********************************\r\n");
	sprintf(msg, "Report for %s\r\n", noItemName);
	write(msg);
	write("**********************************\r\n");

	sprintf(msg, "Summary: \r\n\tFound %d %s\r\n\tFound %d %s\r\n", 
		noConnections->size(), noItemName, yesConnections->size(), yesItemName); write(msg);
	sprintf(msg, "\r\nDetails (%s):\r\n", noItemName); write(msg);
	write(noConnections);
	sprintf(msg, "\r\nDetails (%s):\r\n", yesItemName); write(msg);
	write(yesConnections);

	write("----------------**----------------\r\n");

	closeFile();
}
WA_CollisionHandler::WA_CollisionHandler(WaypointAnalyser *owner)
{
	_owner = owner;
}
void WA_CollisionHandler::newRound()
{
	_nextCollisionDetectionReportTime = 0;
}
bool WA_CollisionHandler::detectCollision()
{
	bool collisionDetected = _owner->getNavigation().detectCollision();

	if(collisionDetected && (g_world->getTime() >= _nextCollisionDetectionReportTime))
	{
		char msg[256];
		Vector position = _owner->getFreeNavigation().getDestination();
		sprintf(msg, "%.2f: Collision detected while trying to move to waypoint '%d' (%.2f, %.2f, %.2f) \n", 
			g_world->getTime(), _owner->getWaypointNavigation().getDestinationWaypointIndex(), 
			position.x, position.y, position.z);
		util::debug::printToServer(msg);

		_nextCollisionDetectionReportTime = g_world->getTime() + 1.0;
	}

	return collisionDetected;
}
WA_UnreachablesState::WA_UnreachablesState(WaypointAnalyser *owner)
{
	_owner = owner;
	_isOn = false;
	_startIndex = _currentIndex = _endIndex = -1;
}
void WA_UnreachablesState::newRound()
{
	if(_isOn)
	{
		if(_currentIndex != _startIndex)
			_currentIndex--; // Try again the waypoint which was interrupted...
		_owner->getWaypointNavigation().stop();
	}
}
void WA_UnreachablesState::start(int startIndex, int endIndex)
{
	util::debug::printToServer("Started finding and validating unreachables...\n");

	_isOn = true;
	_startIndex = startIndex;
	_currentIndex = startIndex;
	_endIndex = endIndex;
	_unreachables.clear();
	_reachables.clear();
	memset(_hits, 0, sizeof(bool)* MAX_WAYPOINTS);

	_owner->getWaypointNavigation().stop();
	_owner->navigation->waypointHitHandler = this;
}
void WA_UnreachablesState::stop()
{
	_owner->getWaypointNavigation().stop();
	_isOn = false;
	_owner->navigation->waypointHitHandler = NULL;
}
void WA_UnreachablesState::waypointHit(int index)
{
	if(_isOn)
		if(index >= _startIndex)
			pushReachable(index);
}
void WA_UnreachablesState::pushReachable(int index)
{
	if(_hits[index])
		return;

	_hits[index] = true;
	char msg[128];
	_reachables.push_back(index);
	sprintf(msg, "\tSuccessfully reached waypoint %d\n", index);
	util::debug::printToServer(msg);
}
void WA_UnreachablesState::update()
{
	if(!_isOn)
		return;

	char msg[256];
	if(_owner->getWaypointNavigation().reachedDestinationWaypoint())
	{
		while(_hits[_currentIndex] && (_currentIndex <= _endIndex))
			_currentIndex++;
		
		if(_currentIndex <= _endIndex)
			_owner->getWaypointNavigation().moveToWaypoint(_currentIndex);
		else
		{
			char msg[256];
			sprintf(msg, "Finished: found %d unreachables.\n", _unreachables.size());
			util::debug::printToServer(msg);
					
			WA_Results results;
			results.toFile("vUnreachables", "validated unreachables", "validated reachables", 
				&_unreachables, &_reachables);
			stop();
		}
	}
	if(_owner->collisionHandler->isStuck())
	{
		sprintf(msg, "WARNING: Agent got stuck while trying to reach waypoint %d!\n", _currentIndex);
		util::debug::printToServer(msg);
		_unreachables.push_back(_currentIndex);
		_owner->getWaypointNavigation().stop();
		_currentIndex++;
	}
}
WA_ConnectionsState::WA_ConnectionsState(WaypointAnalyser *owner)
{
	_owner = owner;
	_state = States::end;
	_startIndex = _endIndex = -1;
}
void WA_ConnectionsState::newRound()
{
	if(_state == States::pending)
	{
		_state = _pendingState;

		char msg[256];
		sprintf(msg, " Resuming validation...\r\n");
		write(msg);
	}

	if(_state == States::moveToWaypoint)
	{
		_owner->getWaypointNavigation().stop();
		_owner->getWaypointNavigation().moveToWaypoint(_currentIndex);
	}
	else if(_state == States::moveToNeighbor)
	{
		_currentNeighborIndex--;
		_owner->getWaypointNavigation().stop();
		_state = States::selectNeighbor;
	}
}
void WA_ConnectionsState::start(int startIndex, int endIndex)
{	
	_state = States::selectWaypoint;
	_startIndex = startIndex;
	_currentIndex = startIndex - 1;
	_endIndex = endIndex;
	_owner->getWaypointNavigation().stop();
	_owner->navigation->waypointHitHandler = this;
	_validConnections.clear();
	_invalidConnections.clear();
	memset(_hits, 0, sizeof(bool)* MAX_WAYPOINTS * MAX_WAYPOINTS);
	_startConnectionIndex = -1;

	_results.openFile("iConnections");
	char msg[256];
	write("**********************************\r\n");
	sprintf(msg, "Report for Invalid Connections\r\n");
	write(msg);
	write("**********************************\r\n");
	write("STARTED...\r\n");
	write("Schema: <wp> -> <ngb> ... {OK | FAILED!}	(<path length>)\r\n\r\n");
}
void WA_ConnectionsState::stop()
{
	_state = States::end;
	_owner->getWaypointNavigation().stop();
	_owner->navigation->waypointHitHandler = NULL;
}

void WA_ConnectionsState::pushConnection(int startIndex, int endIndex)
{
	if(startIndex == endIndex)
		return;
	if(_hits[startIndex][endIndex])
		return;

	_hits[startIndex][endIndex] = true;

	WA_Connection* record = new WA_Connection();
	record->waypoint = startIndex;
	record->neighbor = endIndex;
	_validConnections.push_back(record);
	char msg[64];
	sprintf(msg, "\t%d -> %d ... OK\r\n", startIndex, endIndex);
	write(msg);
}
void WA_ConnectionsState::waypointHit(int index)
{
	if((_state != States::end) && (_state != States::pending))
	{
		if(_startConnectionIndex >= _startIndex)
			pushConnection(_startConnectionIndex, index);
		_startConnectionIndex = index;
	}
}
void WA_ConnectionsState::update()
{
	if((_state == States::end) || (_state == States::pending))
		return;

	char msg[256];

	if((_owner->roundEndTime - g_world->getTime()) < 5) // Less than 5 seconds to end of round?
	{
		_pendingState = _state;
		_state = States::pending;

		sprintf(msg, " Waiting for round to end.\r\n");
		write(msg);
	}

	if(_state == States::selectWaypoint)
	{
		if(_currentIndex < _endIndex)
		{
			_currentIndex++;
			_currentWaypoint = _owner->getWaypointNavigation().getWaypoints()[_currentIndex];
			_currentNeighborIndex = 0;
			_state = States::selectNeighbor;
			
			sprintf(msg, " Started exploring waypoint %d.\r\n", _currentIndex);
			write(msg);
		}
		else // Finished navigating, so, report...
			_state = States::reporting;
	}
	else if(_state == States::selectNeighbor)
	{
		if(_currentNeighborIndex < MAX_PATH_INDEX)
		{
			_currentNeighborWaypointIndex = _currentWaypoint->index[_currentNeighborIndex++];
			while(_hits[_currentIndex][_currentNeighborWaypointIndex] && (_currentNeighborWaypointIndex != -1))
				_currentNeighborWaypointIndex = _currentWaypoint->index[_currentNeighborIndex++];
			if(_currentNeighborWaypointIndex != -1)
			{
				_owner->getWaypointNavigation().moveToWaypoint(_currentIndex);
				_state = States::moveToWaypoint;
			}
			else
				_state = States::selectWaypoint;
		}
		else
			_state = States::selectWaypoint;
	}
	else if(_state == States::moveToWaypoint)
	{
		if(_owner->getWaypointNavigation().reachedDestinationWaypoint())
		{
			_owner->getWaypointNavigation().moveToWaypoint(_currentNeighborWaypointIndex);
			_state = States::moveToNeighbor;
		}
		if(_owner->collisionHandler->isStuck())
		{
			sprintf(msg, "\tStuck while returning to waypoint %d. Trying again...\r\n", _currentIndex);
			write(msg);
			_owner->getWaypointNavigation().stop();
			_owner->getWaypointNavigation().moveToWaypoint(_currentIndex);
		}
	}
	else if(_state == States::moveToNeighbor)
	{
		if(_owner->getWaypointNavigation().reachedDestinationWaypoint())
		{
			pushConnection(_currentIndex, _currentNeighborWaypointIndex);
			_state = States::selectNeighbor;
		}
		
		if(_owner->collisionHandler->isStuck())
		{
			sprintf(msg, "\t%d -> %d ... FAILED!\r\n", _currentIndex, _currentNeighborWaypointIndex);
			write(msg);
			
			WA_Connection* record = new WA_Connection();
			record->waypoint = _currentIndex;
			record->neighbor = _currentNeighborWaypointIndex;
			_invalidConnections.push_back(record);

			_owner->getWaypointNavigation().stop();
			_state = States::selectNeighbor;
		}
	}
	else if(_state == States::reporting)
	{
		report();
		_state = States::end;
	}
}
void WA_ConnectionsState::report()
{
	char msg[256];
	write("FINISHED.\r\n");
	sprintf(msg, "\r\nSummary: \r\n\tFound %d invalid connections\r\n\tFound %d valid connections.\r\n", 
		_invalidConnections.size(), _validConnections.size()); write(msg);
	sprintf(msg, "\r\nDetails (invalid connections):\r\n"); write(msg);
	_results.write(&_invalidConnections);
	_results.closeFile();

	list<WA_Connection*>::iterator it;
	for(it = _invalidConnections.begin(); it != _invalidConnections.end(); it++)
		delete (*it);
	for(it = _validConnections.begin(); it != _validConnections.end(); it++)
		delete (*it);
}
void WA_ConnectionsState::write(const char* msg)
{
	_results.write(msg);
}